#include <iostream>
// #include <Eigen/Dense>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Vector3.h>

int main (int argc, char** argv)
{   
   
    tf2::Quaternion q(0.02226,  0.43968, 0.360423, 0.822363);
    double y, p, r;

    std::cout << "tf2::Quaternion:\n" << q.x() << "  " << q.y() << "  "<< q.z() << "  " << q.w() << std::endl;;

    tf2::Matrix3x3(q).getEulerYPR(y, p, r);
    std::cout << "getEulerYPR(rpy):\n" << r * 180 / M_PI << "  " << p * 180 / M_PI << "  "<< y * 180 / M_PI << std::endl;;


    tf2::Matrix3x3(q).getRPY(r, p, y);
    std::cout << "getRPY(rpy):\n" << r * 180 / M_PI << "  " << p * 180 / M_PI << "  "<< y * 180 / M_PI << std::endl;;

    // 

    double roll, pitch, yaw;
    roll = 30 / 180. * M_PI;
    pitch = 45 / 180. * M_PI;
    yaw = 60 / 180. * M_PI;


    q.setEuler(yaw, pitch, roll);
    std::cout << "q.setEuler:\n" << q.x() << "  " << q.y() << "  "<< q.z() << "  " << q.w() << std::endl;;
    tf2::Matrix3x3(q).getEulerYPR(y, p, r);
    std::cout << "getEulerYPR(rpy):\n" << r * 180 / M_PI << "  " << p * 180 / M_PI << "  "<< y * 180 / M_PI << std::endl;;

    q.setEulerZYX(yaw, pitch, roll);
    std::cout << "q.setEulerZYX:\n" << q.x() << "  " << q.y() << "  "<< q.z() << "  " << q.w() << std::endl;;
    tf2::Matrix3x3(q).getEulerYPR(y, p, r);
    std::cout << "getEulerYPR(rpy):\n" << r * 180 / M_PI << "  " << p * 180 / M_PI << "  "<< y * 180 / M_PI << std::endl;;

    q.setRPY(roll, pitch, yaw);
    std::cout << "q.setEuler:\n" << q.x() << "  " << q.y() << "  "<< q.z() << "  " << q.w() << std::endl;;
    tf2::Matrix3x3(q).getEulerYPR(y, p, r);
    std::cout << "getEulerYPR(rpy):\n" << r * 180 / M_PI << "  " << p * 180 / M_PI << "  "<< y * 180 / M_PI << std::endl;;


    return 0;
}